﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    ///
    /// System_Car1st
    ///

    public class System_Car1st : System_SimplifiedPlanarPhysicalRobot
    {
        private double L;

        public double GetWheelbase()
        {
            return L;
        }

        public override bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);

            TextReader tr = new StreamReader(fileName);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            string[] separators = new string[] { "<wheel_base>", "</wheel_base>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.L = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            tr.Close();
            return true;
        }

        public override bool Propagate(double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            double fy = u[0];
            double ds = u[1] * duration;
            /////////////////////////////////////////////////
            double x = q_start[0];
            double y = q_start[1];
            double th = q_start[2];
            double d = L / Math.Tan(fy);
            double turn = ds / d;
            double vdx = d * Math.Cos(th - Math.PI * 0.5);
            double vdy = d * Math.Sin(th - Math.PI * 0.5);
            double vdex = vdx * Math.Cos(turn) - vdy * Math.Sin(turn);
            double vdey = vdx * Math.Sin(turn) + vdy * Math.Cos(turn);
            double dvx = vdex - vdx;
            double dvy = vdey - vdy;
            double dth = turn;

            q_end[0] = x + dvx;
            q_end[1] = y + dvy;
            q_end[2] = th + dth;
            while (q_end[2] > Math.PI) q_end[2] -= MotionPlanningUtils.PI_double;
            while (q_end[2] < -Math.PI) q_end[2] += MotionPlanningUtils.PI_double;
            step_cost = Math.Abs(ds);

            if (q_end[0] < q_min[0] || q_end[0] > q_max[0])
                return false;
            if (q_end[1] < q_min[1] || q_end[1] > q_max[1])
                return false;
            if (CollisionFree(q_end) == false)
                return false;

            return true;
        }

        public override double Distance2_Topological(double[] x1, double[] x2)
        {
            double diff_q1 = (x1[0] - x2[0]) * q_scale[0];
            double diff_q2 = (x1[1] - x2[1]) * q_scale[1];
            double diff_q3 = Math.Abs(x1[2] - x2[2]);

            while (diff_q3 > MotionPlanningUtils.PI_double)
                diff_q3 -= MotionPlanningUtils.PI_double;
            if (diff_q3 > Math.PI)
                diff_q3 = MotionPlanningUtils.PI_double - diff_q3;
            diff_q3 *= q_scale[2];
            return diff_q1 * diff_q1 + diff_q2 * diff_q2 + diff_q3 * diff_q3;
        }

        public override string ToString()
        {
            return "\n\t<System_Type>Robotics_Car1st</System_Type>" +
                    base.ToString();
        }
    }
}
